#include "ubt_state/ubt_state_api.h"
#include "ubt_state/ubt_state_task_adjust.h"
#include "ubt_state/ubt_state_task_charge.h"
#include "ubt_state/ubt_state_task_charge_ex.h"
#include "ubt_state/node.h"

namespace ubt_state
{

ITask *CTaskChargeEx::CreateTask(ITaskCb *pTaskCb, FixePathTaskInfo *pTaskInfo, int index, int reportIndex, int chargeTime, std::string chargeType)
{
    CTaskChargeEx *pTask = new CTaskChargeEx();
    if (!pTask->Init(pTaskCb, pTaskInfo, index, reportIndex, chargeTime, chargeType))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskChargeEx::~CTaskChargeEx()
{
    RCLCPP_INFO(node->get_logger(),"_curLoop=%d", _curLoop);
    _CleanCurLoop();
}

bool CTaskChargeEx::Init(ITaskCb *pTaskCb, FixePathTaskInfo *pTaskInfo, int index, int reportIndex, int chargeTime, std::string chargeType)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s,chargeTime=%d",
             pTaskInfo->taskId.c_str(),chargeTime);

    node->declare_parameter("agv_type", "");
     _agvType = node->get_parameter("agv_type").as_string();

    _pTaskCb = pTaskCb;
    _pTaskInfo = pTaskInfo;
    _index = index;
    _reportIndex = reportIndex;
    _state = ubt_task_state_idle;
    _curLoop = -1;

    if (_agvType.substr(0, 5) == "t1000")
    {
        _maxLoop = 1;
    }
    else
    {
        _maxLoop = 3;
    }

    isCharging = false;
    _chargeType = chargeType;

    //_pTaskArr
    _curTask = NULL;
    _curTaskIndex = -1;
    _isChargePileFail = false;
    _chargeTime = chargeTime;
    _lastChargeLines = _CalculateLastChargeLine(*pTaskInfo);
    return true;
}

StateStatus *CTaskChargeEx::GetStateStatus()
{
    return _pTaskCb->GetStateStatus();
}

void CTaskChargeEx::NotifyTaskEvent(std::string taskEvent)
{
    _pTaskCb->NotifyTaskEvent(taskEvent);
}

void CTaskChargeEx::NotifyTaskStateAndStage(int state, int stage, int stateReason, std::string stateReasonDesc)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] NotifyTaskStateAndStage ==================== start ====================");
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] NotifyTaskStateAndStage  _curLoop=%d _curTaskIndex=%d  state=%d stage=%d stateReason=%d stateReasonDesc=%s",
             _curLoop, _curTaskIndex, state, stage, stateReason, stateReasonDesc.c_str());
    do
    {
        if (stage == ubt_task_stage_charge)
        {
            isCharging = true;
        }
        if (state != ubt_task_state_closed)
        {
            _pTaskCb->NotifyTaskStateAndStage(state, stage, stateReason, stateReasonDesc);
            break;
        }
        if (stateReason != 0)
        {
            if ((_state != ubt_task_state_stopping) &&
                    (!isCharging || (_chargeType == "uslam" && isCharging)) &&
                    (_curLoop <= _maxLoop))
            {
                _StartNextLoop();
                break;
            }
            _pTaskCb->NotifyTaskStateAndStage(state, stage, stateReason, stateReasonDesc);
            break;
        }
        if (_curTaskIndex >= (_pTaskArr.size() - 1))
        {
            _pTaskCb->NotifyTaskStateAndStage(state, stage, 0, "");
            break;
        }
        _StartNextTask();
        _pTaskCb->NotifyTaskStateAndStage(_curTask->GetState(), _curTask->GetStage(), 0, "");
    }
    while (false);
    
}

bool CTaskChargeEx::Start()
{
    RCLCPP_INFO(node->get_logger()," _curLoop=%d", _curLoop);
    if (_state != ubt_task_state_idle)
    {
        return false;
    }
    _state = ubt_task_state_running;
    _StartNextLoop();
    return true;
}

void CTaskChargeEx::Pause()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_curTask == NULL)
    {
        return ;
    }
    if (_state == ubt_task_state_running)
    {
        _state = ubt_task_state_pause;
        _curTask->Pause();
    }
}

void CTaskChargeEx::Continue()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_curTask == NULL)
    {
        return ;
    }
    if (_state == ubt_task_state_pause)
    {
        _state = ubt_task_state_running;
        if (_curTask->GetState() == ubt_task_state_idle)
        {
            _curTask->Start();
        }
        else
        {
            _curTask->Continue();
        }
    }
}

void CTaskChargeEx::Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"CTaskChargeEx::Stop _state=%d reason=%d", _state, reason);
    if(reason == ubt_task_result_usr_stopbtn)//拍下急停，停止报可恢复故障
    {
        _isChargePileFail = false;
    }
    if (_state >= ubt_task_state_stopping || _curTask == NULL)
    {
        return;
    }
    _state = ubt_task_state_stopping;
    _curTask->Stop(reason);
}

void CTaskChargeEx::OnTimer()
{
    if (_curTask!=NULL)
    {
        _curTask->OnTimer();
    }
}

void CTaskChargeEx::OnMsg(int msgType, void *pvMsg)
{
    if (_curTask!=NULL)
    {
        _curTask->OnMsg(msgType, pvMsg);
    }
}

ubt_task_type CTaskChargeEx::GetType()
{
    return ubt_task_type_chargeex;
}

ubt_task_state CTaskChargeEx::GetState()
{
    if(_curTask==NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        return ubt_task_state_idle;
    }
    ubt_task_state taskState = _curTask->GetState();
    if ((taskState == ubt_task_state_idle) && (_state == ubt_task_state_pause))
    {
        taskState = ubt_task_state_pause;
    }
    return taskState;
}

ubt_task_stage CTaskChargeEx::GetStage()
{
    if(_curTask==NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        return _isChargePileFail ? ubt_task_stage_recharge_fail : ubt_task_stage_run;
    }
    return _isChargePileFail ? ubt_task_stage_recharge_fail : _curTask->GetStage();
}

std::string CTaskChargeEx::GetActionType()
{
    if(_curTask==NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        return "charge";
    }
    else if(_chargeTime != -1)
    {
        return "online-charge";
    }
    return _curTask->GetActionType();
}

void CTaskChargeEx::GetIndex(int& index, int& reportIndex)
{
    if(_curTask!=NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        _curTask->GetIndex(index, reportIndex);
    }

}

void CTaskChargeEx::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    if(_curTask!=NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        _curTask->GetStopInfo(stopReason, stopReasonDesc);
    }
}

void CTaskChargeEx::GetLogs(std::vector<TaskLogItem>& logs)
{
    if(_curTask!=NULL)//为了解决充电路上，拍急停会节点挂死问题
    {
        _curTask->GetLogs(logs);
    }
}

void CTaskChargeEx::_CleanCurLoop()
{
    std::vector<ITask *>::iterator iter;
    for (iter = _pTaskArr.begin(); iter != _pTaskArr.end(); ++iter)
    {
        delete *iter;
    }
    _pTaskArr.clear();
    _curTask = NULL;
    _curTaskIndex = -1;
}


bool CTaskChargeEx::_StartNextLoop()
{
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] _StartNextLoop start");
    _CleanCurLoop();
    ITask *pTask;
    int iRet;
    bool bRet = true;
    Line lastLine = _pTaskInfo->path[_pTaskInfo->path.size() - 1];
    Point lastPoint = lastLine.points[1];
    geometry_msgs::msg::Pose2D poseDstAdjust,poseDst, poseLmt, poseAvoid;
    _curLoop++;

    //超过次数就报可恢复故障，并停止任务
    if(_curLoop > _maxLoop)
    {
        _isChargePileFail = true;
        return false;
    }
    poseDstAdjust.x = 0;
    poseDstAdjust.y = 0;
    poseDstAdjust.theta = 10;

    poseLmt.x = 0.003;
    poseLmt.y = 0.003;
    poseLmt.theta = 0.02;

    poseAvoid.x = 0;
    poseAvoid.y = 0;
    poseAvoid.theta = 0;

    poseDst.x = lastPoint.x;
    poseDst.y = lastPoint.y;
    poseDst.theta = lastPoint.t;

    if(_agvType.substr(0, 5) == "u1000" || _agvType.substr(0, 4) == "u600")
    {
        // 获取进入充电桩方式
        if (_chargeType == "uslam") // && _lastChargeLines
        {
            if (_curLoop > 0 && _curLoop <= _maxLoop)  // should nav to charge
            {
                auto lineInfos = &_lastChargeLines;
                pTask = CTaskNav2::CreateTask(this, _pTaskInfo->taskId, lineInfos, ubt_task_stage_recharge, 0, 0, 0, _curLoop);
                _pTaskArr.push_back(pTask);
            }
            pTask = CTaskAdjust::CreateTask(this, _pTaskInfo->taskId, ubt_task_stage_recharge, true, 0, 0);
        }
        else
        {
            pTask = CTaskAdjust::CreateTask(this, _pTaskInfo->taskId, ubt_task_stage_recharge, 150000,
                                        ubt_proto_adjust::ubt_adjust_pose_qrcode, poseDstAdjust, poseLmt,0.1, 0.05, 0,
                                        0, 0, "charge-adjust");
        }
        _pTaskArr.push_back(pTask);
        
        if(_chargeTime != -1)
        {
            pTask = CTaskCharge::CreateTask(this, _pTaskInfo->taskId, poseDst, 0.2, 0, 0, _chargeType, _chargeTime);
        }
        else
        {
            pTask = CTaskCharge::CreateTask(this, _pTaskInfo->taskId, poseDst, 0.2, 0, 0, _chargeType);
        }
        _pTaskArr.push_back(pTask);
    }
    else
    {
        if(_chargeTime != -1)
        {
            pTask = CTaskCharge::CreateTask(this, _pTaskInfo->taskId, poseDst, 0.2, 0, 0, "", _chargeTime);
        }
        else
        {
            pTask = CTaskCharge::CreateTask(this, _pTaskInfo->taskId, poseDst, 0.2, 0, 0, "");
        }
        _pTaskArr.push_back(pTask);
    }

    _StartNextTask();
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] _StartNextLoop end.");
    return true;
}

void CTaskChargeEx::_StartNextTask()
{
    RCLCPP_INFO(node->get_logger(),"CTaskChargeEx::_StartNextTask _curTaskIndex: %d", _curTaskIndex);
    _curTaskIndex++;
    if (_curTaskIndex >= _pTaskArr.size())
    {
        RCLCPP_ERROR(node->get_logger(),"[CTaskChargeEx] _StartNextTask : index is out of bounds!");
        return ;
    }
    _curTask = _pTaskArr[_curTaskIndex];
    if (_state == ubt_task_state_running)
    {
        _curTask->Start();
    }
}

std::vector<ubt_state::Line> CTaskChargeEx::_CalculateLastChargeLine(FixePathTaskInfo taskInfo)
{
    std::vector<ubt_state::Line> lines;

    RCLCPP_INFO(node->get_logger(),"CTaskChargeEx::_CalculateLastChargeLine : FixePathTaskInfo : task id: %s, path size: %d ", taskInfo.taskId.c_str(), taskInfo.path.size());

    if (taskInfo.path.back().points.size() < 2)
    {
        RCLCPP_ERROR(node->get_logger(),"[CTaskChargeEx] _CalculateLastChargeLine : points size < 2, size is %d", (int)(taskInfo.path.end()->points.size()));
        return std::vector<ubt_state::Line> {};
    }
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] _CalculateLastChargeLine, taskInfo.path.end()->points size: %d", taskInfo.path.back().points.size());
    auto point = taskInfo.path.back().points[1];
    RCLCPP_INFO(node->get_logger(),"[CTaskChargeEx] _CalculateLastChargeLine: (%d, %f, %f)", point.id, point.x, point.y);
    std::vector<Point> points;
    points.push_back(point);
    points.push_back(point);
    auto line = taskInfo.path.back();
    line.points = points;
    lines.push_back(line);
    
    return lines;
}

}



